/**
* @file ObsFollowPlanner.h
* @author
* @brief 沿边规划
* @version 1.0
* @date 2021-08-6
*
* @copyright Copyright (c) 2021
*
*/

#pragma once

#include "data_struct/base/DataSet.h"
#include "geometry_msgs/msg/pose_stamped.hpp"
#include "lib/pure_pursuit_controller.h"

namespace behavior_controller
{

class ObsFollowPlanner
{
public:
    /**
     * @brief 沿边规划的构造函数类
     * @return
     */
    ObsFollowPlanner() {};

    /**
     * @brief 更新沿边距离
     * @return
     */
    void UpdateObsDistance(double dis) { dis_ = dis; };

    /**
     * @brief 沿边规划的启动运行函数
     * @return
     */
    void run(double &linear_vel, double &angular_vel, bool &finished,
             std::shared_ptr<PurePursuitControllerD> &controllerF);

    void followRoom(double &linear_vel, double &angular_vel, bool &finished);

    void followPartRoom(double &linear_vel, double &angular_vel, bool &finished,
                        std::shared_ptr<PurePursuitControllerD> &controllerF);

    void followVirtualWall(double &linear_vel, double &angular_vel, bool &finished,
                           std::shared_ptr<PurePursuitControllerD> &controllerF);

    /**
     * @brief 沿障碍物时的PID调节函数
     * @return
     */
    void pid_adjustment(double &linear_vel, double &angular_vel);

    void setOnline(bool flag) { online_flag = flag; };

    bool isOnline() { return online_flag; };
    double last_dis_p = 0; // 记录上一次的距离偏差，用于计算微分环节的偏差之差
    double total_err = 0;
    //PoseF start_pose;
    geometry_msgs::msg::PoseStamped start_pose;

private:
    std::vector<std::vector<geometry_msgs::msg::PoseStamped>> *wallPtr = nullptr;
    double dis_; //传感器反馈的沿边距离
    bool room_init = false;
    bool control_flag = false;
    bool online_flag = false;
};

//static
extern ObsFollowPlanner obs_follow_planner; //实例化沿障碍物类
} // namespace behavior_controller